#!/usr/bin/env python
import roslib
roslib.load_manifest('turtlesim')
 
import rospy
import rospkg
import sys
 
from geometry_msgs.msg import Twist
from select import select

if sys.platform == 'win32':
    import msvcrt
else:
    import termios
    import tty

pub_4, pub_6, pub_8 = None, None, None
settings_ = None

def getKey(settings, timeout):
    if sys.platform == 'win32':
        # getwch() returns a string on Windows
        key = msvcrt.getwch()
    else:
        tty.setraw(sys.stdin.fileno())
        # sys.stdin.read() returns a string on Linux
        rlist, _, _ = select([sys.stdin], [], [], timeout)
        if rlist:
            key = sys.stdin.read(1)
        else:
            key = ''
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key
def saveTerminalSettings():
    if sys.platform == 'win32':
        return None
    return termios.tcgetattr(sys.stdin)

def restoreTerminalSettings(old_settings):
    if sys.platform == 'win32':
        return
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)

def stop_turtle():
    twist = Twist()
    twist.linear.x = 0.0
    twist.angular.z = 0.0
    
    pub_4.publish(twist)
    pub_6.publish(twist)
    pub_8.publish(twist)
    restoreTerminalSettings(settings_)

 
def teleop_turtle(speed_lim=0.5):
    # 初始化节点
    rospy.init_node('teleop_turtle')
 
    # 发布cmd_vel话题的发布者
    pub_4 = rospy.Publisher('/turtle_4/cmd_vel', Twist, queue_size=1)
    pub_6 = rospy.Publisher('/turtle_6/cmd_vel', Twist, queue_size=1)
    pub_8 = rospy.Publisher('/turtle_8/cmd_vel', Twist, queue_size=1)
 
    # 设置turtle的速度
    twist_4 = Twist()
    twist_6 = Twist()
    twist_8 = Twist()
 
    # 设置键盘监听
    rospy.on_shutdown(lambda: stop_turtle())
    settings_ = saveTerminalSettings()
    key_timeout = rospy.get_param("~key_timeout", 0.5)
    twist_4.linear.x = speed_lim * rospy.get_param('~linear', 1.0)
    twist_4.angular.z = speed_lim * rospy.get_param('~angular', 1.0)

    twist_6.linear.x = twist_4.linear.x
    twist_6.angular.z = twist_4.angular.z

    twist_8.linear.x = twist_4.linear.x
    twist_8.angular.z = twist_4.angular.z

    # 键盘映射
    move_dict_4 = {
        'i': (1, 0),
        'o': (1, 1),
        'j': (0, 1),
        'm': (-1, 1),
        'u': (0, -1),
        'n': (1, -1),
    }

    move_dict_6 = {
        'i': (1, 0),
        'o': (1, 1),
        'j': (0, 1),
        'm': (-1, 1),
        'u': (0, -1),
        'n': (1, -1),
    }

    move_dict_8 = {
        'i': (1, 0),
        'o': (1, 1),
        'j': (0, 1),
        'm': (-1, 1),
        'u': (0, -1),
        'n': (1, -1),
    }     
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        key = input("Press a key (i, j, m, u, n, o): ") # python2
        # key = getKey(settings_, key_timeout)           
        print("input: ", key)
        twist_4.linear.x = 0.0
        twist_4.angular.z = 0.0
        twist_6.linear.x = 0.0
        twist_6.angular.z = 0.0        
        twist_8.linear.x = 0.0
        twist_8.angular.z = 0.0
        if key in move_dict_4:
            twist_4.linear.x = move_dict_4[key][0] * twist_4.linear.x
            twist_4.angular.z = move_dict_4[key][1] * twist_4.angular.z
        if key in move_dict_6:
            twist_6.linear.x = move_dict_6[key][0] * twist_6.linear.x
            twist_6.angular.z = move_dict_6[key][1] * twist_6.angular.z
        if key in move_dict_8:
            twist_8.linear.x = move_dict_8[key][0] * twist_8.linear.x
            twist_8.angular.z = move_dict_8[key][1] * twist_8.angular.z
        pub_4.publish(twist_4)
        pub_6.publish(twist_6)
        pub_8.publish(twist_8)
        rate.sleep()
 
if __name__ == '__main__':
    try:
        teleop_turtle()
    except rospy.ROSInterruptException:
        pass
    finally:
        stop_turtle()
        print("Teleop stopped.")